#!/usr/bin/env python3
import rospy
import argparse
from nav_msgs.msg import Odometry
from sensor_msgs.msg import Imu
from geometry_msgs.msg import TwistStamped

# 服务
from uuv_gazebo_ros_plugins_msgs.srv import GetFloat

class ParameterMonitor:
    def __init__(self, args):
        rospy.init_node('parameter_monitor', anonymous=True)
        self.args = args

        # 服务
        if args.odom:
            rospy.wait_for_service('/rexrov/get_fluid_density')
            rospy.wait_for_service('/rexrov/get_damping_scaling')
            self.get_density = rospy.ServiceProxy('/rexrov/get_fluid_density', GetFloat)
            self.get_damping = rospy.ServiceProxy('/rexrov/get_damping_scaling', GetFloat)

        # 订阅
        if args.odom:
            self.last_odom_time = None
            self.last_vx = self.last_vy = self.last_vz = 0.0
            rospy.Subscriber('/rexrov/pose_gt', Odometry, self.odom_callback)
        if args.imu:
            rospy.Subscriber('/rexrov/imu', Imu, self.imu_callback)
        if args.dvl:
            # DVL话题类型通常为geometry_msgs/TwistStamped
            rospy.Subscriber('/rexrov/dvl', TwistStamped, self.dvl_callback)

        print("参数监控器已启动，观测内容：")
        if args.odom:
            print("  - 位置/速度/加速度/流体参数")
        if args.imu:
            print("  - IMU")
        if args.dvl:
            print("  - DVL")
        print("=" * 60)

    def odom_callback(self, msg):
        # 位置
        x = msg.pose.pose.position.x
        y = msg.pose.pose.position.y
        z = msg.pose.pose.position.z
        # 线速度
        vx = msg.twist.twist.linear.x
        vy = msg.twist.twist.linear.y
        vz = msg.twist.twist.linear.z
        # 角速度
        wx = msg.twist.twist.angular.x
        wy = msg.twist.twist.angular.y
        wz = msg.twist.twist.angular.z

        # 线加速度（差分法）
        t = msg.header.stamp.to_sec()
        if self.last_odom_time is not None:
            dt = t - self.last_odom_time
            if dt > 0:
                ax = (vx - self.last_vx) / dt
                ay = (vy - self.last_vy) / dt
                az = (vz - self.last_vz) / dt
            else:
                ax = ay = az = 0.0
        else:
            ax = ay = az = 0.0

        # 服务获取参数
        try:
            density = self.get_density().data
            damping = self.get_damping().data
        except:
            density = damping = float('nan')

        print(f"[ODOM] t={t:.2f}s 位置: x={x:.3f}, y={y:.3f}, z={z:.3f}")
        print(f"       线速度: vx={vx:.3f}, vy={vy:.3f}, vz={vz:.3f}")
        print(f"       线加速度: ax={ax:.3f}, ay={ay:.3f}, az={az:.3f}")
        print(f"       角速度: wx={wx:.3f}, wy={wy:.3f}, wz={wz:.3f}")
        print(f"       流体密度: {density:.1f} 阻尼缩放: {damping:.3f}")
        print("-" * 40)

        self.last_odom_time = t
        self.last_vx = vx
        self.last_vy = vy
        self.last_vz = vz

    def imu_callback(self, msg):
        t = msg.header.stamp.to_sec()
        ax = msg.linear_acceleration.x
        ay = msg.linear_acceleration.y
        az = msg.linear_acceleration.z
        gx = msg.angular_velocity.x
        gy = msg.angular_velocity.y
        gz = msg.angular_velocity.z
        print(f"[IMU] t={t:.2f}s 线加速度: ax={ax:.3f}, ay={ay:.3f}, az={az:.3f}")
        print(f"      角速度: gx={gx:.3f}, gy={gy:.3f}, gz={gz:.3f}")
        print("-" * 40)

    def dvl_callback(self, msg):
        t = msg.header.stamp.to_sec()
        vx = msg.twist.linear.x
        vy = msg.twist.linear.y
        vz = msg.twist.linear.z
        print(f"[DVL] t={t:.2f}s 线速度: vx={vx:.3f}, vy={vy:.3f}, vz={vz:.3f}")
        print("-" * 40)

    def run(self):
        rospy.spin()

if __name__ == '__main__':
    parser = argparse.ArgumentParser(description="UUV参数与状态监控")
    parser.add_argument('--odom', action='store_true', help='观测里程计/位置/速度/加速度/流体参数')
    parser.add_argument('--imu', action='store_true', help='观测IMU数据')
    parser.add_argument('--dvl', action='store_true', help='观测DVL数据')
    args = parser.parse_args()

    # 如果没有参数，默认全部观测
    if not (args.odom or args.imu or args.dvl):
        args.odom = args.imu = args.dvl = True

    try:
        monitor = ParameterMonitor(args)
        monitor.run()
    except rospy.ROSInterruptException:
        pass 